/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.templates.tshirtcannon.TShirtCannon;
import java.util.TimerTask;
import java.util.Timer;
import edu.wpi.first.wpilibj.Relay;


public class RobotDemo extends IterativeRobot {
    private Joystick m_leftStick = new Joystick(1);
    private Joystick m_rightStick = new Joystick(2);
    private Joystick m_gigaware = new Joystick(3);
    private Victor m_frontLeftMotor;
    private Victor m_frontRightMotor;
    private Victor m_rearLeftMotor;
    private Victor m_rearRightMotor;
    private RobotDrive m_drivetrain;
    private JoyDrive m_joyDrive;
    private Compressor m_compressor = new Compressor(kDigitalSlot, 1, kDigitalSlot, 1);
    private Solenoid m_fillValve = new Solenoid(kPhumaticSlot, 1);
    private Solenoid m_dumpValve = new Solenoid(kPhumaticSlot, 4);
    private Solenoid m_fireValve1 = new Solenoid(kPhumaticSlot, 2);
    private Solenoid m_fireValve2 = new Solenoid(kPhumaticSlot, 3);
    private Relay m_tShirtCannonLight = new Relay(kDigitalSlot, 2);
    private TShirtCannon m_tShirtCannon = new TShirtCannon(m_compressor, m_fillValve, m_dumpValve, m_fireValve1, m_fireValve2, m_tShirtCannonLight, m_leftStick, m_rightStick, m_gigaware);
    private static final int kDigitalSlot = 4;
    private static final int kPhumaticSlot = 7;
    private boolean m_compButtonEnable = true;

    public void robotInit() {
        m_frontLeftMotor = new Victor(kDigitalSlot, 2);
        m_frontRightMotor = new Victor(kDigitalSlot, 3);
        m_rearLeftMotor = new Victor(kDigitalSlot, 1);
        m_rearRightMotor = new Victor(kDigitalSlot, 4);
        m_drivetrain = new Team997Drive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor, m_leftStick, m_rightStick, .05, .3);
        m_joyDrive = new JoyDrive(m_drivetrain, m_leftStick, m_rightStick, m_gigaware, false);//Start a timer task to update the SmartDashboard.
        ((Team997Drive) m_drivetrain).setReverseDeadband(.04);
        ((Team997Drive) m_drivetrain).setCurve(.2);
        Timer dashTimer = new Timer();
        dashTimer.schedule(new TimerTask() {
            public void run() {
                updateSmartDashboard();
            }
        }, 0, 200);
        System.out.println("Build Complete!");
    }

    public void disabledInit() {
        m_tShirtCannon.disable();
    }

    public void autonomousInit() {
    }

    public void autonomousPeriodic() {
    }

    public void autonomousContinuous() {
    }

    public void teleopInit() {
        m_tShirtCannon.disable();
    }

    public void teleopPeriodic() {
    }

    public void teleopContinuous() {
        if (m_drivetrain instanceof Team997Drive) {
            Team997Drive drivetrain = (Team997Drive) m_drivetrain;
            if (m_leftStick.getRawButton(7) || m_gigaware.getRawButton(7)) {
                drivetrain.setMotorMax(0.3);
            } else if (m_rightStick.getRawButton(7) || m_gigaware.getRawButton(8)) {
                drivetrain.setMotorMax(0.5);
            }
        }
        
        m_tShirtCannon.update();

        m_joyDrive.update();
    }

    private void updateSmartDashboard() {
        if (m_drivetrain instanceof Team997Drive) {
            Team997Drive drivetrain = (Team997Drive) m_drivetrain;
            SmartDashboard.log(drivetrain.getCurve(), "Curve");
            SmartDashboard.log(drivetrain.getReverseDeadband(), "Reverse Deadband");
            SmartDashboard.log(drivetrain.getDeadband(), "Deadband");
            SmartDashboard.log(drivetrain.getMotorMax(), "Motor Max");
            SmartDashboard.log(drivetrain.getLeftMotorValue(), "Left Motor Value");
            SmartDashboard.log(drivetrain.getRightMotorValue(), "Right Motor Value");
        }
        
        SmartDashboard.log(m_tShirtCannon.getCurrentStateName(), "Cannon State");
        SmartDashboard.log(m_tShirtCannon.getCompressorState(), "Compressor");
        SmartDashboard.log(m_tShirtCannon.getPressureSwitchState(), "Pressure Switch");
        SmartDashboard.log(m_tShirtCannon.getFillValveState(), "Fill Valve");
        SmartDashboard.log(m_tShirtCannon.getDumpValveState(), "Dump Valve");
        SmartDashboard.log(m_tShirtCannon.getFireValve1State(), "Left Fire Valve");
        SmartDashboard.log(m_tShirtCannon.getFireValve2State(), "Right Fire Valve");
        SmartDashboard.log(m_tShirtCannon.getLightState(), "Light State");
        SmartDashboard.log(m_tShirtCannon.getTimerSeconds(), "Cannon Timer");
        SmartDashboard.log(m_tShirtCannon.getEnabled(1), "Enabled Left");
        SmartDashboard.log(m_tShirtCannon.getEnabled(2), "Enabled Right");
    }
}